#include "Includes.h"
//////////////////////////////////////////////////////////////////////////////////////
#define Init_OK_key vexRT[Btn7U]
int pre_Init_val=0;
/////
void pre_Init()
{
	writeDebugStream("pre_init\n");
	Choose_Field();
	Choose_Role();
	Led_Show();
	while( !Init_OK_key )
	{
		//writeDebugStream("%d\n",vexRT[Ch2]);
		Force_UpDown_Handle();
		Turning_Arm_Handle();

		Claw_OpenClose_Handle();
	}
	while(Init_OK_key);
	SensorValue[LED]=led_on; //on
	Init_Enc_Arm_Sensor_dst();
	Claw_Statue_Func(Claw_Open);
	Move_Stop();

	while( !Init_OK_key )
	{
		Force_UpDown_Handle();
		Turning_Arm_Handle();
	}
	while(Init_OK_key);

	SensorValue[LED]=led_off;//off

	SemaphoreInitialize(sem_Rise_Updown);
	SemaphoreInitialize(sem_Turning_Arm);

	Rise_UpDown_Func(-15);
}
////////////////////////////////
task usercontrol()
{
	writeDebugStream("usercontrol\n");
	if(pre_Init_val==0)
	{
		pre_Init();
		pre_Init_val=1;
	}
	while(1)
	{
		Robot_Move(vexRT[Ch3],vexRT[Ch1],125,85);

		Rise_UpDown_Handle();
		Force_UpDown_Handle();
		Claw_OpenClose_Handle();
		Turning_Arm_Handle();
		Cylinder_Hook_Handle();
	}
}
